#include "Motor.h"
#include "ti/driverlib/dl_gpio.h"
#include "ti_msp_dl_config.h"





static volatile Encoder Encoder_A;
static volatile Encoder Encoder_B;


/******************************电机初始化********************************************/
void Motor_Init(void)
{
    DL_GPIO_setPins(MotorDr_STBY_PORT, MotorDr_STBY_PIN);
    // //编码器引脚外部中断
	// NVIC_ClearPendingIRQ(ENCODER_INT_IRQN);
	// NVIC_EnableIRQ(ENCODER_INT_IRQN);

    // //定时器中断
	// NVIC_ClearPendingIRQ(TIMER_0_INST_INT_IRQN);
	// NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);

}
/**********************************************************************************/

/**********************************************************************************/
int Motor_Get_Encoder(uint8_t dir)
{
	if( !dir )
		return Encoder_A.Obtained_Get_Encoder_Count;

	return Encoder_B.Obtained_Get_Encoder_Count;
}

void GROUP1_IRQHandler(void)
{
    uint32_t gpio_interrup = 0;

	//获取中断信号
	gpio_interrup = DL_GPIO_getEnabledInterruptStatus(ENCODER_PORT,ENCODER_E1A_PIN|ENCODER_E1B_PIN|ENCODER_E2A_PIN|ENCODER_E2B_PIN);

    // encoderA
	if((gpio_interrup & ENCODER_E1A_PIN)==ENCODER_E1A_PIN)
	{
		if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E1B_PIN))
		{
			Encoder_A.Should_Get_Encoder_Count--;
		}
		else
		{
			Encoder_A.Should_Get_Encoder_Count++;
		}
	}
	else if((gpio_interrup & ENCODER_E1B_PIN)==ENCODER_E1B_PIN)
	{
		if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E1A_PIN))
		{
			Encoder_A.Should_Get_Encoder_Count++;
		}
		else
		{
			Encoder_A.Should_Get_Encoder_Count--;
		}
	}

	// encoderB
	if((gpio_interrup & ENCODER_E2A_PIN)==ENCODER_E2A_PIN)
	{
		if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E2B_PIN))
		{
			Encoder_B.Should_Get_Encoder_Count--;
		}
		else
		{
			Encoder_B.Should_Get_Encoder_Count++;
		}
	}
	else if((gpio_interrup & ENCODER_E2B_PIN)==ENCODER_E2B_PIN)
	{
		if(!DL_GPIO_readPins(ENCODER_PORT,ENCODER_E2A_PIN))
		{
			Encoder_B.Should_Get_Encoder_Count++;
		}
		else
		{
			Encoder_B.Should_Get_Encoder_Count--;
		}
	}
	DL_GPIO_clearInterruptStatus(ENCODER_PORT,ENCODER_E1A_PIN|ENCODER_E1B_PIN|ENCODER_E2A_PIN|ENCODER_E2B_PIN);
}
uint8_t TIM0_counter =0;
extern PID_Struct Motor_PID_left;
extern PID_Struct Motor_PID_Right;
//电机编码器脉冲计数
void TIMER_0_INST_IRQHandler(void)
{
	//编码器速度计算
	if( DL_TimerG_getPendingInterrupt(TIMER_0_INST) == DL_TIMER_IIDX_ZERO )
	{
        TIM0_counter++;
        if(TIM0_counter == 4)
        {
            /* 两个电机安装相反，所以编码器值也要相反 */
            Encoder_A.Obtained_Get_Encoder_Count = Encoder_A.Should_Get_Encoder_Count;
            Encoder_B.Obtained_Get_Encoder_Count = -Encoder_B.Should_Get_Encoder_Count;

            /* 编码器计数值清零 */
            Encoder_A.Should_Get_Encoder_Count = 0;
            Encoder_B.Should_Get_Encoder_Count = 0;
            Motor_PID_left.PIDCounter++;
            Motor_PID_Right.PIDCounter++;
            Motor_PID_left.GetValue  = Encoder_A.Obtained_Get_Encoder_Count;
            Motor_PID_Right.GetValue = Encoder_B.Obtained_Get_Encoder_Count;
            TIM0_counter = 0;
        }
        
	}
}
int SpeedL(void)
{
    return Motor_Get_Encoder(0);
}
int SpeedR(void)
{
    return Motor_Get_Encoder(1);
}

/**********************************************************************************/


/******************************电机A控制********************************************/
void A_Motor_forward(void)
{
    DL_GPIO_setPins(MotorDr_AIN1_PORT, MotorDr_AIN1_PIN);
    DL_GPIO_clearPins(MotorDr_AIN2_PORT, MotorDr_AIN2_PIN);
}

void A_Motor_backward(void)
{
    DL_GPIO_clearPins(MotorDr_AIN1_PORT, MotorDr_AIN1_PIN);
    DL_GPIO_setPins(MotorDr_AIN2_PORT, MotorDr_AIN2_PIN);
}

void A_Motor_stop(void)
{
    DL_GPIO_clearPins(MotorDr_AIN1_PORT, MotorDr_AIN1_PIN);
    DL_GPIO_clearPins(MotorDr_AIN2_PORT, MotorDr_AIN2_PIN);
}

void A_Motor_speed(int speed)       //speed-->[0,1000]
{
    speed = 1000 - speed;              //value为1000时占空比为0，所以做这样的处理
    DL_TimerG_setCaptureCompareValue(PWM_A_INST,speed,GPIO_PWM_A_C1_IDX);   //PWA使用TimerG
}

void A_Motor_ctrl(uint8_t direction, int speed)
{
    if (direction == forward)
    {
        A_Motor_forward();
        A_Motor_speed(speed);
    }
    else if (direction == backward)
    {
        A_Motor_backward();
        A_Motor_speed(speed);
    }
    else if (direction == stop)
        A_Motor_stop() ;
}
/**********************************************************************************/

/******************************电机B控制********************************************/
void B_Motor_forward(void)
{
    DL_GPIO_setPins(MotorDr_BIN1_PORT, MotorDr_BIN1_PIN);
    DL_GPIO_clearPins(MotorDr_BIN2_PORT, MotorDr_BIN2_PIN);
}

void B_Motor_backward(void)
{
    DL_GPIO_clearPins(MotorDr_BIN1_PORT, MotorDr_BIN1_PIN);
    DL_GPIO_setPins(MotorDr_BIN2_PORT, MotorDr_BIN2_PIN);
}

void B_Motor_stop(void)
{
    DL_GPIO_clearPins(MotorDr_BIN1_PORT, MotorDr_BIN1_PIN);
    DL_GPIO_clearPins(MotorDr_BIN2_PORT, MotorDr_BIN2_PIN);
}

void B_Motor_speed(int speed)       //speed-->[0,100]
{
    speed = 1000 - speed;              //value为1000时占空比为0，所以做这样的处理
    DL_TimerA_setCaptureCompareValue(PWM_B_INST,speed,GPIO_PWM_B_C1_IDX);   //PWB使用TimerA
}

void B_Motor_ctrl(uint8_t direction, int speed)
{
    if (direction == forward)
    {
        B_Motor_forward();
        B_Motor_speed(speed);
    }
    else if (direction == backward)
    {
        B_Motor_backward();
        B_Motor_speed(speed);
    }
    else if (direction == 3)
        B_Motor_stop() ;
}

/**********************************************************************************/

/******************************小车控制********************************************/

void car_ctrl(uint8_t direction, int speed)
{
    if(direction == forward)
    {
        A_Motor_ctrl(forward, speed);
        B_Motor_ctrl(forward, speed);
    }
    else if(direction == backward)
    {
        A_Motor_ctrl(backward, speed);
        B_Motor_ctrl(backward, speed);
    }
    else if(direction == stop)
    {
        A_Motor_ctrl(stop, 0);
        B_Motor_ctrl(stop, 0);
    }
}
/**********************************************************************************/


